/*
 * encoder.c
 *
 *  Created on: 2022年11月18日
 *      Author: paper
 */

#include "encoder.h"
float low_pass = 0.7;//低通滤波器的系数，取值范围通常在 0 到 1 之间。系数越接近 1，滤波效果越强，输出越平滑，但响应速度越慢；系数越接近 0，滤波效果越弱，输出越接近原始值

//面朝车头顺时针转动编码器为正方向
int encoder_bldc_f = 0;//左无刷电机速度
int encoder_bldc_b = 0;//右无刷电机速度
int encoder_bldc_f_total;
int encoder_bldc_b_total;
float encoder_bdc = 0;    // 有刷电机脉冲计数
float encoder_bdc_total = 0;
int16 encoder_bdc_get = 0;
float encoder=0;
void encoder_get(void) {
    int16 encoder_bldc_f_get=motor_value.receive_left_speed_data;
    int16 encoder_bldc_b_get=-motor_value.receive_right_speed_data;
    encoder_bldc_f = encoder_bldc_f * low_pass + encoder_bldc_f_get * (1 - low_pass);
    encoder_bldc_f_total += encoder_bldc_f;
    encoder_bldc_b = encoder_bldc_b * low_pass + encoder_bldc_b_get * (1 - low_pass);
    encoder_bldc_b_total += encoder_bldc_b;

//    encoder_bldc_f = motor_value.receive_left_speed_data;//左无刷电机速度
//    encoder_bldc_b = -motor_value.receive_right_speed_data;//右无刷电机速度
    if(motor_value.receive_left_speed_data>4000||motor_value.receive_left_speed_data<-4000)motor_value.receive_left_speed_data=(int16)encoder_bldc_f;
    if(motor_value.receive_right_speed_data>4000||motor_value.receive_right_speed_data<-4000)motor_value.receive_left_speed_data=(int16)encoder_bldc_b;
    // 有刷电机脉冲计数
    encoder_bdc_get = encoder_get_count(TIM4_ENCODER);
    // 编码器 低通滤波
    encoder_bdc = encoder_bdc * low_pass + encoder_bdc_get * (1 - low_pass);
    encoder_bdc_total += encoder_bdc;
    encoder=encoder_bdc;
    encoder_clear_count(TIM4_ENCODER);
    if(encoder_bdc>1450||encoder_bdc<-1450)encoder_bdc=encoder;//7000pwm下有刷编码器速度不到1400
}
